
#include "CombAlg.h"
#include "Psi.h"

CombAlg::CombAlg(Grid* grid,  float** psi_ext, int n, int m){


    alpha = ppmts->getAlpha();
    beta_pl = ppmts->getBetaPl();
    psi_g = ppmts->getPsiG();
    r0 = ppmts->getR0();
    I = ppmts->getI();

    right_r = grid->getRightR();
    left_r = grid->getLeftR();
    up_z = grid->getUpZ();
    down_z = grid->getDownZ();

    size_n = n;
    size_m = m;
    omega = 1.539;
    e = ppmts->getPrecision();
    h_r = grid->getHR();
    h_r_2 = h_r * h_r;
    h_z = grid->getHZ();
    h_z_2 = h_z * h_z;

    A = 2 * ( (1/(h_r * h_r))+ (1/(h_z * h_z)) );

    D =  1/(h_z*h_z);
    E = 1/(h_z*h_z);

    //coordinates
    left_r_pl = ppmts->getPlasmaLeftR();
    right_r_pl = ppmts->getPlasmaRightR();
    up_z_pl = ppmts->getPlasmaUpZ();
    down_z_pl = ppmts->getPlasmaDownZ();

    //indexes
    left_r_g = psi->left_r_g;
    right_r_g = psi->right_r_g;
    up_z_g = psi->up_z_g;
    down_z_g = psi->down_z_g;


    psist.SetSize(size_n, size_m);
    jphi_s.SetSize(size_n, size_m);

    //jphi initial condition

    for (int i = 0; i < size_n; i++) {
        for (int j = 0; j < size_m; j++) {
            if ((i == size_n / 2) && (j == size_m / 2)) {
                jphi_s(i, j) = ppmts->getI() / (h_r*h_z);

            } else {
                jphi_s(i, j) = 0;
    }
        }
    }


    plog->writeInFile("jphi_s", jphi_s);


    for (int i = 0; i < size_n; i++) {
        for (int j = 0; j < size_m; j++) {
            psist(i, j) = psi_ext[left_r_g + i][down_z_g + j];
        }
}

    plog->writeInFile("psist_s", psist);

}

float CombAlg::B(int i){
    
    return ( (1 / ((left_r_pl + i * h_r) * 2 * h_r)) + 1 / (h_r * h_r) );

}

float CombAlg::C(int i) {
    return ( - (1 / ((left_r_pl + i * h_r) * 2 * h_r)) + 1 / (h_r * h_r) );
}

float CombAlg::F(int i, int j){
   return (left_r_pl + i * h_r)*8*(Pi*Pi/c_light)*jphi_alt(i, j);
};

void CombAlg::solve() {
    Matrix tmp_psipl = bound;
    Matrix psipl = bound;

    for (int k = 0; k < 500; k++) {
        tmp_psipl = psipl;
        for (int j = 1; j < psipl.ColNo()-1; j ++) {
          for (int i = 1; i < psipl.RowNo()-1; i++)  {
             psipl(i, j) = (1/A)*(B(i)*tmp_psipl(i-1,j) + C(i)*tmp_psipl(i+1, j) + D*tmp_psipl(i,j-1) + E*tmp_psipl(i, j+1) + F(i,j));
          }
        }

    printf("\nk: %d sub:  %f", k, err->getValue(psipl, tmp_psipl));
    }
    
    plog->writeInFile("comb_res", psipl);

};

void CombAlg::solve_sor() {
    Matrix tmp_psipl = bound;
    Matrix psipl = bound;
    Matrix psipl_hat = bound;
    float miscal;
    int k = 0;
    do {
        k+= 1;
        //Zeidel part
        tmp_psipl = psipl;
        for (int j = 1; j < psipl.ColNo() - 1; j++) {
            for (int i = 1; i < psipl.RowNo() - 1; i++) {
                psipl(i, j) = (1 / A)*(B(i) * psipl(i - 1, j) + C(i) * tmp_psipl(i + 1, j)
                        + D * psipl(i, j - 1) + E * tmp_psipl(i, j + 1) + F(i, j));
            }
        }
        // SOR part
        psipl_hat = psipl;
        for (int j = 1; j < psipl.ColNo() - 1; j++) {
            for (int i = 1; i < psipl.RowNo() - 1; i++) {
                psipl(i,j) = psipl(i,j) + omega* (psipl_hat(i,j) - psipl(i,j));
            }
        }
         

        miscal = err->getValue(psipl, tmp_psipl);
        printf("\nk: %d sub: %f", k, miscal);

    } while (miscal > 0.01*e && k < 500 );
   
    plog->writeInFile("comb_sor_res", psipl);

    }
    
Matrix CombAlg::solve_sor2(Matrix m_bound, Matrix m_jphi) {
    Matrix tmp_psipl = m_bound;
    Matrix psipl = m_bound;
    Matrix psipl_hat = m_bound;
 

    float miscal;
    int k = 0;
    do {
        k += 1;
        //Zeidel part
        tmp_psipl = psipl;
        
        for (int j = 1; j < psipl.ColNo() - 1; j++) {
            for (int i = 1; i < psipl.RowNo() - 1; i++) {
                psipl(i, j) = (1 / A)*(B(i) * psipl(i - 1, j) + C(i) * tmp_psipl(i + 1, j)
                        + D * psipl(i, j - 1) + E * tmp_psipl(i, j + 1) + (left_r_pl + i * h_r)*8 * (Pi * Pi / c_light) * m_jphi(i, j) );
            }
        }
        
        // SOR part
        psipl_hat = psipl;
        for (int j = 1; j < psipl.ColNo() - 1; j++) {
            for (int i = 1; i < psipl.RowNo() - 1; i++) {
                psipl(i, j) = psipl(i, j) + omega * (psipl_hat(i, j) - psipl(i, j));
            }
        }


        miscal = err->getValue(psipl, tmp_psipl);
        //miscal = 100;
        printf("\nk: %d sub: %f", k, miscal);

    } while (miscal > 0.01 * e && k < 500);

    plog->writeInFile("comb_sor_res", psipl);

    return psipl;
};


Matrix CombAlg::get_psipl_on_g(Matrix m_jphi) {
    Matrix psipl_l;
    psipl_l.SetSize(size_n, size_m);
    float res = 0;
    float r, z = 0;
    float ri, zi = 0;
    for (int j = 0; j < size_m; j++) {
        for (int i = 0; i < size_m; i++) {
            if (i == 0 || i == size_n - 1 || j == 0 || j == size_m - 1) {
                r = left_r + i * h_r;
                z = down_z + j * h_z;
                /////////////////// integral calculation
                res = 0;
                ri, zi = 0;
                for (int m = 0; m < size_m; m++) {
                    zi = down_z + m * h_z;
                    for (int n = 0; n < size_n; n++) {
                        ri = left_r + n * h_r;
                        res = res + G(r, z, ri, zi) * m_jphi(n, m);
                    }
                }
                res = res * h_r * h_z;
                psipl_l(i,j) = res;
            }
        }
    }
    return psipl_l;
};

Matrix CombAlg::get_jphi(Matrix psipl) {
    Matrix jphi_l;
    jphi_l.SetSize(size_n, size_m); 
    float r, z = 0;
    float sum = 0;
    float gamma = 0;

    //innormalized jphi calculation
    for (int j = 0; j < size_m; j++) {
        z = down_z + j*h_z;
        for (int i = 0; i < size_n; i++) {

            r = left_r + i*h_r;
            //check for Omega area (ext current excluded)

            
            if (psipl(i,j) + psist(i,j) > psi_g) {
                jphi_l(i,j) = pow((psipl(i, j) + psist(i, j) - psi_g), alpha)*
                        ((r / r0) * beta_pl + (r0 / r)*
                        (1 - beta_pl));
                sum = sum + jphi_l(i, j);
            } else {
                jphi_l(i, j) = 0;
            }
        }
    }

    sum = sum * h_r*h_z;
    gamma = I / sum;
    printf("gamma: %f", gamma);
    for (int j = 0; j < size_m; j++) {
        for (int i = 0; i < size_n; i++) {
            jphi_l(i, j) = jphi_l(i,j) * gamma;
        }
    }
    return jphi_l;

};